#include "Puma6C.h"


void PumaPack::ConvertToDeg() {
	if(!rad) return;
	rad = false;

	ang1 = RadToDeg(ang1);
	ang2 = RadToDeg(ang2);
	ang3 = RadToDeg(ang3);
	ang4 = RadToDeg(ang4);
	ang5 = RadToDeg(ang5);
}

void PumaPack::ConvertToRad() {
	if(rad) return;
	rad = true;

	ang1 = DegToRad(ang1);
	ang2 = DegToRad(ang2);
	ang3 = DegToRad(ang3);
	ang4 = DegToRad(ang4);
	ang5 = DegToRad(ang5);
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
void Puma6C::CalculateAng3(float* pMatrix, PumaPack* res) {
	float c1, s1, c4, s4, a23, x, y;
	for(int i=0; i<8; ++i) {
		c1 = cosf(res[i].ang1);
		s1 = sinf(res[i].ang1);
		c4 = cosf(res[i].ang4);
		s4 = sinf(res[i].ang4);

		x = ( pMatrix[0] + s1 * s4 ) / ( c1 * c4 );
		y = -pMatrix[2] / c4;
		a23 = atan2(y, x);
		if(a23 < 0.0f) a23 += PI2;
		res[i].ang3 = a23 - res[i].ang2;
		if(res[i].ang3 < 0.0f) res[i].ang3 += PI2;
	}
}

void Puma6C::CalculateQ2(float* pMatrix, PumaPack* res) {
	float c4, c1, c2;
	for(int i=0; i<8; ++i) {
		c4 = cosf(res[i].ang4);
		c2 = cosf(res[i].ang2);
		c1 = cosf(res[i].ang1);

		res[i].q2 = ( c4 * ( pMatrix[12] - q4 * pMatrix[0] ) - c1 * q3 * pMatrix[2] ) / (c1 * c2 * c4);
	}
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
void Puma6C::CalculateAng2(float* pMatrix, PumaPack* res) {
	float c1, c4, s1, s4, arg;
	for(int i=0; i<8; i+=2) {
		c1 = cosf(res[i].ang1);
		s1 = sinf(res[i].ang1);
		c4 = cosf(res[i].ang4);
		s4 = sinf(res[i].ang4);
		
		arg = -( c1 * c4 * (pMatrix[14] - q4 * pMatrix[2] - q1) + q3 * (pMatrix[0] + s1 * s4) )
			/ ( c4 * (pMatrix[12] - q4 * pMatrix[0]) - c1 * q3 * pMatrix[2] );
		res[i].ang2 = atan(arg);
		res[i+1].ang2 = res[i].ang2 + PI;
		if(res[i].ang2 < 0.0f) res[i].ang2 += PI2;
	}
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
void Puma6C::CalculateAng5(float* pMatrix, PumaPack* res) {
	float c1, s1, c4, y, x;
	for(int i=0; i<8; i+=2) {
		c1 = cosf(res[i].ang1);
		s1 = sinf(res[i].ang1);
		c4 = cosf(res[i].ang4);
		y = (s1 * pMatrix[8] - c1 * pMatrix[9]) / c4;
		x = (c1 * pMatrix[5] - s1 * pMatrix[4]) / c4;
		res[i].ang5 = atan2(y, x);
		if(res[i].ang5 < 0.0f) res[i].ang5 += PI2;
		res[i+1].ang5 = atan2(y, x);
		if(res[i+1].ang5 < 0.0f) res[i+1].ang5 += PI2;
	}
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
void Puma6C::CalculateAng1(float* pMatrix, PumaPack* res) {
	float arg = (pMatrix[13] - q4 * pMatrix[1]) / (pMatrix[12] - q4 * pMatrix[0]);
	res[0].ang1 = atanf(arg);
	res[4].ang1 = res[0].ang1 + PI;
	if(res[0].ang1 < 0.0f) res[0].ang1 += PI2;
	res[1] = res[2] = res[3] = res[0];
	res[5] = res[6] = res[7] = res[4];
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
void Puma6C::CalculateAng4(float* pMatrix, PumaPack* res) {
	float c1 = cosf(res[0].ang1);
	float s1 = sinf(res[0].ang1);
	res[0].ang4 = asinf(c1 * pMatrix[1] - s1 * pMatrix[0]);
	res[2].ang4 = PI - res[0].ang4;
	if(res[0].ang4 < 0.0f) res[0].ang4 += PI2;
	res[1] = res[0];
	res[3] = res[2];

	c1 = cosf(res[4].ang1);
	s1 = sinf(res[4].ang1);
	res[4].ang4 = asinf(c1 * pMatrix[1] - s1 * pMatrix[0]);
	res[6].ang4 = PI - res[4].ang4;
	if(res[4].ang4 < 0.0f) res[4].ang4 += PI2;
	res[5] = res[4];
	res[7] = res[6];
}

bool Puma6C::Reverse(float *pMatrix, PumaPack *res) {
	CalculateAng1(pMatrix, res);
	CalculateAng4(pMatrix, res);
	CalculateAng5(pMatrix, res);
	CalculateAng2(pMatrix, res);
	CalculateQ2(pMatrix, res);
	CalculateAng3(pMatrix, res);
	for(int i=0; i<8; ++i) {
		/*res[i].ang1 = RadToDeg(res[i].ang1);
		res[i].ang2 = RadToDeg(res[i].ang2);
		res[i].ang3 = RadToDeg(res[i].ang3);
		res[i].ang4 = RadToDeg(res[i].ang4);
		res[i].ang5 = RadToDeg(res[i].ang5);*/
		res[i].rad = true;
		res[i].ConvertToDeg();
	}
	//DebTest(res);
	return true;
}

PumaPack Puma6C::CloseToPrev(PumaPack prev, PumaPack *res, int& index) {
	float c, arg, cmin;
	int ind;
	ind = 0;
	cmin = 10000.0f;
	for(int i=0; i<8; ++i) {
		c = 0.0f;
		arg = abs(prev.ang1 - res[i].ang1);
		if(arg > 180.0f) arg = 360.0f - arg;
		c += arg;
		arg = abs(prev.ang2 - res[i].ang2);
		if(arg > 180.0f) arg = 360.0f - arg;
		c += arg;
		arg = abs(prev.ang3 - res[i].ang3);
		if(arg > 180.0f) arg = 360.0f - arg;
		c += arg;
		arg = abs(prev.ang4 - res[i].ang4);
		if(arg > 180.0f) arg = 360.0f - arg;
		c += arg;
		arg = abs(prev.ang5 - res[i].ang5);
		if(arg > 180.0f) arg = 360.0f - arg;
		c += arg;
		arg = (prev.q2 - res[i].q2);
		if(arg < 0) arg = 0;
		c += arg;
		if(cmin > c) {
			cmin = c;
			ind = i;
		}
	}
	index = ind;
	return res[ind];
}

void Puma6C::DebTest(PumaPack* res) {
	float c1, c2, c3, c4, c5, s1, s2, s3, s4, s5;
	for(int i=0; i<8; ++i) {
		c1 = cosf(res[i].ang1);
		s1 = sinf(res[i].ang1);
		
		c2 = cosf(res[i].ang2);
		s2 = sinf(res[i].ang2);

		c3 = cosf(res[i].ang3);
		s3 = sinf(res[i].ang3);

		c4 = cosf(res[i].ang4);
		s4 = sinf(res[i].ang4);

		c5 = cosf(res[i].ang5);
		s5 = sinf(res[i].ang5);
	}
}